#include <string.h>	
#include <unistd.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <sys/time.h>
#include "control.h"
#include "controlUtils.cpp"

using namespace std;
static int drone_number = -1;
static int max_swarm_number = 0;
static string server_ip = "127.0.0.1";
static int listeningPort = 30002;

int main(int argc, char **argv) {	
	std::cout<<"请输入集群总数量:";
	std::cin>>max_swarm_number;
	ControlConn conn(server_ip.c_str(), listeningPort);
	if (conn.fd == -1)
	{
		perror("listen socket initialize fail...");
		exit(0);
	}
	Client ct(conn.fd, &conn.addr);

	vector<DroneState> swarmsStateMsgs;
	vector<ControlMsg> controlVec;
	swarmsStateMsgs.resize(max_swarm_number);
	controlVec.resize(max_swarm_number);

	for(int i=0;i<max_swarm_number;++i){
		//无人机编号初始化
		controlVec[i].droneNumber = i;
		//控制位置更新
		controlVec[i].position = vector<double>{1.0, 1.0, 1.0};
		//控制模式更新
		controlVec[i].mode = "OFFBOARD";
	}

	ct.refresh();// 发送"REFRESH",更新集群状态信息
	while(true){
		//接收集群状态信息
		int len;
		len = ct.recvMsg(swarmsStateMsgs);
        // 此处写你的计算逻辑 
		// 根据集群状态信息 计算并更新controlVec

		// for(int i=0;i<max_swarm_number;++i){
		// 	//控制位置更新
		// 	controlVec[i].position = vector<double>{1.0, 1.0, 1.0};
		// 	//控制模式更新
		// 	controlVec[i].mode = "OFFBOARD";
		// }
		// 发送控制信息
		ct.sendMsg(controlVec);

		if (len < 0){
			perror("read");
			break;
		}
		else if (len == 0)
		{
			printf("server disconneted...\n");
			break;
		}
		else{
			sleep(0.5);
		}
	}
	close(conn.fd);
	return 0;
}
